/**
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
  * @file       usb_task.c/h
  * @brief      通过USB串口与上位机通信
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Jun-24-2024     Penguin         1. done

  @verbatim
  =================================================================================

  =================================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2024 Polarbear*************************
*/

#include "usb_task.h"

#include <stdbool.h>
#include <string.h>

#include "CRC8_CRC16.h"
#include "cmsis_os.h"
#include "data_exchange_center.h"
#include "macro_typedef.h"
#include "usb_device.h"
#include "usb_typdef.h"
#include "usbd_cdc_if.h"
#include "usbd_conf.h"
#include "referee.h"
#include "vision_task.h"

#if INCLUDE_uxTaskGetStackHighWaterMark
uint32_t usb_high_water;
#endif

#define USB_TASK_CONTROL_TIME 1 // ms

#define USB_OFFLINE_THRESHOLD 100 // ms
#define USB_CONNECT_CNT 10

// clang-format off
// #define SEND_DURATION_Imu         5   // ms
// #define SEND_DURATION_Debug       5   // ms
#define SEND_DURATION_RobotInfo   10  // ms
// #define SEND_DURATION_Pid         10  // ms
#define SEND_DURATION_AllRobotHp  10  // ms
#define SEND_DURATION_GameStatus  10  // ms
// #define SEND_DURATION_RobotMotion 10  // ms
#define SEND_DURATION_Vision       10  // ms
#define SEND_DURATION_Simulation   10  // ms
// clang-format on

#define USB_RX_DATA_SIZE 256 // byte
#define USB_RECEIVE_LEN 200  // byte
#define HEADER_SIZE 4        // byte
#define ROUND_TO_4DEC(v) (roundf((v) * 10000.0f) / 10000.0f)
#define CheckDurationAndSend(send_name)                                                \
    do                                                                                 \
    {                                                                                  \
        if ((HAL_GetTick() - LAST_SEND_TIME.##send_name) >= SEND_DURATION_##send_name) \
        {                                                                              \
            LAST_SEND_TIME.##send_name = HAL_GetTick();                                \
            UsbSend##send_name##Data();                                                \
        }                                                                              \
    } while (0)

static uint8_t USB_RX_BUF[USB_RX_DATA_SIZE];

Imu_t *IMU;
SolveAns *solveAns;

// 判断USB连接状态用到的一些变量
static bool USB_OFFLINE = true;
static uint32_t RECEIVE_TIME = 0;
static uint32_t LATEST_RX_TIMESTAMP = 0;
static uint32_t CONTINUE_RECEIVE_CNT = 0;

// 数据发送结构体
static SendDataRobotInfo_s SEND_DATA_ROBOT_INFO;
static SendDataAllRobotHp_s SEND_DATA_ALL_ROBOT_HP;
static SendDataGameStatus_s SEND_DATA_GAME_STATUS;
static SendDataVision_s SEND_DATA_VISION;

// 数据接收结构体
static ReceiveDataRobotCmd_s RECEIVE_ROBOT_CMD_DATA;
static ReceiveDataVision_s RECEIVE_VISION_DATA;
static ReceiveDataSimulation_s RECEIVE_SIMULATION_DATA;

// 发送数据间隔时间
typedef struct
{
    uint32_t RobotInfo;
    uint32_t AllRobotHp;
    uint32_t GameStatus;
    uint32_t Vision;
} LastSendTime_t;
static LastSendTime_t LAST_SEND_TIME;

/*******************************************************************************/
/* Main Function                                                               */
/*******************************************************************************/

static void UsbSendData(void);
static void UsbReceiveData(void);
static void UsbInit(void);

/*******************************************************************************/
/* Send Function                                                               */
/*******************************************************************************/

static void UsbSendRobotInfoData(void);
static void UsbSendAllRobotHpData(void);
static void UsbSendGameStatusData(void);
static void UsbSendVisionData(void);

/*******************************************************************************/
/* Receive Function                                                            */
/*******************************************************************************/

ReceiveDataRobotCmd_s *GetRobotCmdData(void);
ReceiveDataVision_s *GetVisionData(void);
ReceiveDataSimulation_s *GetSimulationData(void);

/******************************************************************/
/* Task                                                           */
/******************************************************************/
uint8_t *sof_address;

/**
 * @brief      USB任务主函数
 * @param[in]  argument: 任务参数
 * @retval     None
 */
void usb_task(void const *argument)
{
    DataExchangeCenterPublish(&RECEIVE_ROBOT_CMD_DATA, ROBOT_CMD_DATA_NAME);
    DataExchangeCenterPublish(&USB_OFFLINE, USB_OFFLINE_NAME);

    MX_USB_DEVICE_Init();

    vTaskDelay(10); // 等待USB设备初始化完成
    UsbInit();

    while (1)
    {
        UsbSendData();
        UsbReceiveData();
        GetRobotCmdData();
        GetSimulationData();
        GetVisionData();

        if (HAL_GetTick() - RECEIVE_TIME > USB_OFFLINE_THRESHOLD)
        {
            USB_OFFLINE = true;
            CONTINUE_RECEIVE_CNT = 0;
        }
        else if (CONTINUE_RECEIVE_CNT > USB_CONNECT_CNT)
        {
            USB_OFFLINE = false;
        }
        else
        {
            CONTINUE_RECEIVE_CNT++;
        }

        vTaskDelay(USB_TASK_CONTROL_TIME);

#if INCLUDE_uxTaskGetStackHighWaterMark
        usb_high_water = uxTaskGetStackHighWaterMark(NULL);
#endif
    }
}

/*******************************************************************************/
/* Main Function                                                               */
/*******************************************************************************/

/**
 * @brief      USB初始化
 * @param      None
 * @retval     None
 */
static void UsbInit(void)
{
    // 订阅数据
    solveAns = GetSolveAns();
    IMU = DataExchangeCenterSubscribe(IMU_NAME); // 获取IMU数据指针

    // 数据置零
    memset(&RECEIVE_ROBOT_CMD_DATA, 0, sizeof(ReceiveDataRobotCmd_s));
    memset(&RECEIVE_VISION_DATA, 0, sizeof(ReceiveDataVision_s));
    memset(&RECEIVE_SIMULATION_DATA, 0, sizeof(ReceiveDataSimulation_s));
    //单个数据特殊处理
    RECEIVE_ROBOT_CMD_DATA.data.stop_gimbal_scan = 1; //取消注释后上电，默认关闭自瞄模式

    // 1.初始化机器人信息数据包
    // 帧头部分
    SEND_DATA_ROBOT_INFO.id = ROBOT_INFO_DATA_SEND_ID;
    // 数据部分
    SEND_DATA_ROBOT_INFO.data.robot_id = 0;
    SEND_DATA_ROBOT_INFO.data.current_hp = 0;
    SEND_DATA_ROBOT_INFO.data.shooter_heat = 0;
    SEND_DATA_ROBOT_INFO.data.team_color = 0;
    SEND_DATA_ROBOT_INFO.data.is_attacked = 0;

    // 2.初始化所有机器人血量数据
    SEND_DATA_ALL_ROBOT_HP.id = ALL_ROBOT_HP_SEND_ID;

    // 3.初始化比赛状态数据
    SEND_DATA_GAME_STATUS.id = GAME_STATUS_SEND_ID;

    // 4.初始化视觉发送数据
    SEND_DATA_VISION.id = VISION_SEND_ID;
}

/**
 * @brief      用USB发送数据
 * @param      None
 * @retval     None
 */
static void UsbSendData(void)
{
    // 发送RobotInfo数据
    CheckDurationAndSend(RobotInfo);
    // 发送AllRobotHp数据
    CheckDurationAndSend(AllRobotHp);
    // 发送GameStatus数据
    CheckDurationAndSend(GameStatus);
    // 发送Vision数据
    CheckDurationAndSend(Vision);
}

/**
 * @brief      USB接收数据
 * @param      None
 * @retval     None
 */
static void UsbReceiveData(void)
{
    static uint32_t len = USB_RECEIVE_LEN;
    static uint8_t *rx_data_start_address = USB_RX_BUF; // 接收数据包时存放于缓存区的起始位置
    static uint8_t *rx_data_end_address;                // 接收数据包时存放于缓存区的结束位置
    uint8_t *sof_address = USB_RX_BUF;

    // 计算数据包的结束位置
    rx_data_end_address = rx_data_start_address + USB_RECEIVE_LEN;
    // 读取数据
    USB_Receive(rx_data_start_address, &len);

    while (sof_address <= rx_data_end_address)
    { // 解析缓冲区中的所有数据包
        while (*(sof_address) != RECEIVE_SOF && (sof_address <= rx_data_end_address))
        {
            sof_address++;
        }
        // 判断是否超出接收数据范围
        if (sof_address > rx_data_end_address)
        {
            break; // 退出循环
        }
        // 检查CRC8校验
        bool crc8_ok = verify_CRC8_check_sum(sof_address, sizeof(FrameHeader_t));
        if (crc8_ok)
        {
            uint8_t data_len = sof_address[1];
            uint8_t data_id = sof_address[2];
            // 检查整包CRC16校验 4: header size, 2: crc16 size
            bool crc16_ok = verify_CRC16_check_sum(sof_address, 4 + data_len + 2);
            if (crc16_ok)
            {
                switch (data_id)
                {
                case ROBOT_CMD_DATA_RECEIVE_ID:
                {
                    // 创建临时结构体存储接收数据
                    ReceiveDataRobotCmd_s temp_data;
                    memcpy(&temp_data, sof_address, sizeof(ReceiveDataRobotCmd_s));

                    // 仅更新非-1字段（以chassis_spin_vel字段为例）
                    if (temp_data.data.chassis_spin_vel != -1.0f)
                    {
                        RECEIVE_ROBOT_CMD_DATA.data.chassis_spin_vel = temp_data.data.chassis_spin_vel;
                    }

                    // 同理处理其他需要过滤的字段（如chassis_speed_k）
                    if (temp_data.data.chassis_speed_k != -1.0f)
                    {
                        RECEIVE_ROBOT_CMD_DATA.data.chassis_speed_k = temp_data.data.chassis_speed_k;
                    }

                    // 更新非过滤字段（如stop_gimbal_scan）
                    RECEIVE_ROBOT_CMD_DATA.data.stop_gimbal_scan = temp_data.data.stop_gimbal_scan;
                }
                break;
                case VISION_RECEIVE_ID:
                {
                    memcpy(&RECEIVE_VISION_DATA, sof_address, sizeof(ReceiveDataVision_s));
                }
                break;
                case SIMULATION_RECEIVE_ID:
                {
                    memcpy(&RECEIVE_SIMULATION_DATA, sof_address, sizeof(ReceiveDataSimulation_s));
                }
                break;
                default:
                    break;
                }
                if (*((uint32_t *)(&sof_address[4])) > LATEST_RX_TIMESTAMP)
                {
                    LATEST_RX_TIMESTAMP = *((uint32_t *)(&sof_address[4]));
                    RECEIVE_TIME = HAL_GetTick();
                }
            }
            sof_address += (data_len + HEADER_SIZE + 2);
        }
        else
        { // CRC8校验失败，移动到下一个字节
            sof_address++;
        }
    }
    // 更新下一次接收数据的起始位置
    if (sof_address > rx_data_start_address + USB_RECEIVE_LEN)
    {
        // 缓冲区中没有剩余数据，下次接收数据的起始位置为缓冲区的起始位置
        rx_data_start_address = USB_RX_BUF;
    }
    else
    {
        uint16_t remaining_data_len = USB_RECEIVE_LEN - (sof_address - rx_data_start_address);
        // 缓冲区中有剩余数据，下次接收数据的起始位置为缓冲区中剩余数据的起始位置
        rx_data_start_address = USB_RX_BUF + remaining_data_len;
        // 将剩余数据移到缓冲区的起始位置
        memcpy(USB_RX_BUF, sof_address, remaining_data_len);
    }
}
/*******************************************************************************/
/* Send Function                                                               */
/*******************************************************************************/

/**
 * @brief 发送机器人信息数据
 * @param duration 发送周期
 */
static void UsbSendRobotInfoData(void)
{
    get_robot_info_id(&SEND_DATA_ROBOT_INFO.data.robot_id);
    get_robot_info_hp(&SEND_DATA_ROBOT_INFO.data.current_hp);
    get_power_heat_data_mode(&SEND_DATA_ROBOT_INFO.data.shooter_heat, 1);
    SEND_DATA_ROBOT_INFO.data.team_color = get_team_color();
    SEND_DATA_ROBOT_INFO.data.is_attacked = 0;

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_ROBOT_INFO, sizeof(SendDataRobotInfo_s));
    USB_Transmit((uint8_t *)&SEND_DATA_ROBOT_INFO, sizeof(SendDataRobotInfo_s));
}

/**
 * @brief 发送全场机器人hp信息数据
 * @param duration 发送周期
 */
static void UsbSendAllRobotHpData(void)
{
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_1_robot_hp, 1);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_2_robot_hp, 2);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_3_robot_hp, 3);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_4_robot_hp, 4);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_5_robot_hp, 5);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_7_robot_hp, 7);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_base_hp, 8);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.red_outpost_hp, 9);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_1_robot_hp, 11);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_2_robot_hp, 12);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_3_robot_hp, 13);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_4_robot_hp, 14);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_5_robot_hp, 15);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_7_robot_hp, 17);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_base_hp, 18);
    get_all_robot_hp_mode(&SEND_DATA_ALL_ROBOT_HP.data.blue_outpost_hp, 19);

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_ALL_ROBOT_HP, sizeof(SendDataAllRobotHp_s));
    USB_Transmit((uint8_t *)&SEND_DATA_ALL_ROBOT_HP, sizeof(SendDataAllRobotHp_s));
}

/**
 * @brief 发送比赛状态数据
 * @param duration 发送周期
 */
static void UsbSendGameStatusData(void)
{

    SEND_DATA_GAME_STATUS.data.game_progress = get_game_status_progress();
    get_game_status_time(&SEND_DATA_GAME_STATUS.data.stage_remain_time);
    SEND_DATA_GAME_STATUS.data.center_buff_down = get_center_buff_down();
    SEND_DATA_GAME_STATUS.data.is_center_buff_down = (bool)get_is_center_buff_down();

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_GAME_STATUS, sizeof(SendDataGameStatus_s));
    USB_Transmit((uint8_t *)&SEND_DATA_GAME_STATUS, sizeof(SendDataGameStatus_s));
}

static void UsbSendVisionData(void)
{
    SEND_DATA_VISION.time_stamp = HAL_GetTick();

    SEND_DATA_VISION.data.detect_color = get_detect_color();
    SEND_DATA_VISION.data.reset_tracker = 0;
    SEND_DATA_VISION.data.reserved = 0;

    // 对IMU数据进行四位小数处理
    SEND_DATA_VISION.data.roll = IMU->roll;
    SEND_DATA_VISION.data.pitch = -IMU->pitch;
    SEND_DATA_VISION.data.yaw = IMU->yaw;

    SEND_DATA_VISION.data.aim_x = solveAns->aim_x;
    SEND_DATA_VISION.data.aim_y = solveAns->aim_y;
    SEND_DATA_VISION.data.aim_z = solveAns->aim_z;

    append_CRC16_check_sum((uint8_t *)&SEND_DATA_VISION, sizeof(SendDataVision_s));
    USB_Transmit((uint8_t *)&SEND_DATA_VISION, sizeof(SendDataVision_s));
}

/*******************************************************************************/
/* Receive Function                                                            */
/*******************************************************************************/

ReceiveDataRobotCmd_s *GetRobotCmdData(void)
{
    return &RECEIVE_ROBOT_CMD_DATA;
}

ReceiveDataVision_s *GetVisionData(void)
{
    return &RECEIVE_VISION_DATA;
}

ReceiveDataSimulation_s *GetSimulationData(void)
{
    return &RECEIVE_SIMULATION_DATA;
}
